package it.unicam.cs.cosy.bioshape.core.collisiondetection.narrowphase;

import it.unicam.cs.cosy.bioshape.core.behaviours.ActiveSite;
import it.unicam.cs.cosy.bioshape.core.components.EntityPair;
import it.unicam.cs.cosy.bioshape.core.components.ShapedEntity;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import vclip.ClosestFeaturesHT;
import vclip.DistanceReport;

public class VClipCollisionDetectorTimeExitCondition implements NarrowPhase {

    // /*
    // * Directions in which the binary search of the first time of contact
    // could
    // * be going at a certain moment
    // */
    // private static final int BACKWARD_DIRECTION = 0;
    //
    // private static final int FORWARD_DIRECTION = 1;

    private List<EntityPair> pairs;

    private ClosestFeaturesHT ht;

    /**
     * Generate a VClip Collision Detector Module
     */
    public VClipCollisionDetectorTimeExitCondition() {
        this.pairs = null;
        this.ht = null; // new ClosestFeaturesHT();
    }

    /**
     * If the interval in which the first time of contact is contained has
     * length less than EPSILON_TIME then the first time of contact is
     * considered to be equal to the rightLimit of the interval
     */
    private static final double EPSILON_TIME = 1E-5;

    // /**
    // * If the lowest distance between two shapes is less than or equal to
    // * EPSILON_DISTANCE then the two shapes are considered touching
    // */
    // private static final double EPSILON_DISTANCE = 0;

    /**
     * Search for the first time of contact. The fundamental hypothesis of this
     * search algorithm is that the given pairs are NOT penetrating at current
     * time, i.e., at time zero of the search interval
     * 
     * @param pairs
     *            a list of possible colliding pairs, presumably calculated by
     *            the broad phase
     * @param delta
     *            the length of the interval for which the first time of contact
     *            is searched for, that is to say, the interval [0, delta]
     * @return the first time of contact among the possible colliding pairs OR
     *         Double.POSITIVE_INFINITY if none of the given pairs is colliding
     *         in the interval [0,delta]
     */
    private static double getFirstTimeOfContact(List<EntityPair> pairs,
            double delta, ClosestFeaturesHT ht) {
        int iterationCount = 0;
        /*
         * The search for ftc could be called even if the broad phase detected
         * no possible colliding pairs, in this case the ftc is "infinite", by
         * convention
         */
        if (pairs.size() == 0)
            return Double.POSITIVE_INFINITY;
        /*
         * Preliminary check - no pair can penetrate at time zero, this is a
         * fundamental hypothesis of the implemented search algorithm
         */
        for (EntityPair pair : pairs) {
            DistanceReport drep = new DistanceReport();
            drep.setFeaturePromotion(true, Math.toRadians(10));
            // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
            // distance
            drep.setMaxClosePairs(1); // closest features
            double distance = getDistanceAfterTime(0, pair, drep, ht);
            if (distance < 0)
                throw new IllegalStateException(
                        "Penetrating pair at the beginning of iteration! Error! Pair = "
                                + pair);
        }
        /*
         * Search for ftc can normally start. Determine the first values for the
         * left, middle and right point of the search interval
         */
        double left = 0;
        double right = delta;
        double middle = delta / 2;
        /*
         * Only at the first iteration of the searching any distance must be
         * calculated
         */
        boolean firstCycle = true;
        /*
         * if the last right limit of the interval is equal to the current right
         * limit then vclip call for the right limit can be avoided
         */
        boolean useLastRight = false;
        /*
         * if the last middle point of the interval is equal to the current
         * right limit then vclip call for the right limit can be avoided
         */
        boolean useLastMiddleAsRight = false;
        /*
         * Control of the cycle
         */
        boolean done = false;
        /*
         * the pairs are divided into those colliding in the right-half of the
         * interval including middle - i.e., [middle, right] -, those colliding
         * in the left-half of the interval - i.e., [left,middle) - and those
         * not colliding at all in the current interval (this can happen only at
         * the first iteration). In any point x of the interval, if the distance
         * between two shapes at x is less than 0, then they are penetrating
         */
        List<EntityPair> leftMiddleCollidingPairs = new ArrayList<EntityPair>();
        List<EntityPair> middleRightCollidingPairs = new ArrayList<EntityPair>();
        System.out.print("FTC-iteration:");
        while (!done) {
            iterationCount++;
            System.out.print(" " + iterationCount);
            /*
             * done will never become true, the cycle exits when an acceptable
             * ftc has been determined OR an IllegalStateException is raised
             * because this is not possible.
             */
            leftMiddleCollidingPairs.clear();
            middleRightCollidingPairs.clear();
            /*
             * Internal cycle to analyze all possibly colliding pairs
             */
            Iterator<EntityPair> iterator = pairs.iterator();
            while (iterator.hasNext()) {
                EntityPair pair = iterator.next();
                /*
                 * save current limits in the pair
                 */
                pair.setLeft(left);
                pair.setMiddle(middle);
                pair.setRight(right);
                DistanceReport drepMiddle = null;
                DistanceReport drepRight = null;
                double distanceMiddle = 0;
                double distanceRight = 0;
                /*
                 * Calculate the distances of the two shapes at time middle and
                 * at time right, possibly saving a call to vclip if one of the
                 * last values can be used
                 */
                if (firstCycle) {
                    drepMiddle = new DistanceReport();
                    drepMiddle.setFeaturePromotion(true, Math.toRadians(10));
                    // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
                    // distance
                    drepMiddle.setMaxClosePairs(1); // closest features
                    distanceMiddle = getDistanceAfterTime(middle, pair,
                            drepMiddle, ht);
                    pair.setDistanceMiddle(distanceMiddle);
                    pair.setDistanceReportMiddle(drepMiddle);
                    drepRight = new DistanceReport();
                    drepRight.setFeaturePromotion(true, Math.toRadians(10));
                    // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
                    // distance
                    drepRight.setMaxClosePairs(1); // closest features
                    distanceRight = getDistanceAfterTime(right, pair,
                            drepRight, ht);
                    pair.setDistanceRight(distanceRight);
                    pair.setDistanceReportRight(drepRight);
                } else {
                    if (useLastRight) {
                        distanceRight = pair.getDistanceRight();
                        drepRight = pair.getDistanceReportRight();
                        drepMiddle = new DistanceReport();
                        drepMiddle
                                .setFeaturePromotion(true, Math.toRadians(10));
                        // drep.setMaxPairDistance(EPSILON_DISTANCE); //
                        // maximal
                        // distance
                        drepMiddle.setMaxClosePairs(1); // closest features
                        distanceMiddle = getDistanceAfterTime(middle, pair,
                                drepMiddle, ht);
                        pair.setDistanceMiddle(distanceMiddle);
                        pair.setDistanceReportMiddle(drepMiddle);
                    } else {
                        if (useLastMiddleAsRight) {
                            distanceRight = pair.getDistanceMiddle();
                            drepRight = pair.getDistanceReportMiddle();
                            pair.setDistanceReportRight(drepRight);
                            pair.setDistanceRight(distanceRight);
                            drepMiddle = new DistanceReport();
                            drepMiddle.setFeaturePromotion(true,
                                    Math.toRadians(10));
                            // drep.setMaxPairDistance(EPSILON_DISTANCE); //
                            // maximal
                            // distance
                            drepMiddle.setMaxClosePairs(1); // closest
                                                            // features
                            distanceMiddle = getDistanceAfterTime(middle, pair,
                                    drepMiddle, ht);
                            pair.setDistanceMiddle(distanceMiddle);
                            pair.setDistanceReportMiddle(drepMiddle);

                        }
                    }
                }
                /*
                 * Analyze all possible cases
                 * 
                 * Case #1: the shapes penetrate at the right limit of the
                 * interval and they do not at the middle point of the interval.
                 * Thus, the contact point must be in the right half of the
                 * current interval. The pair is moved in the corresponding list
                 * (middleRight) and temporarily removed from pairs
                 */
                if (distanceRight <= 0 && distanceMiddle > 0) {
                    middleRightCollidingPairs.add(pair);
                    iterator.remove();
                }
                /*
                 * Case #2: the shapes penetrate at the middle point of the
                 * interval, but not anymore at the right limit. This can happen
                 * when there is rotation: even if they get closer by
                 * translation the rotation makes them to not penetrate anymore.
                 * The real collision must have happened in the left half of the
                 * interval. The pair is moved in the list leftMiddle and
                 * temporarily removed from pairs.
                 */
                if (distanceRight > 0 && distanceMiddle <= 0) {
                    leftMiddleCollidingPairs.add(pair);
                    iterator.remove();
                }
                /*
                 * Case #3 and #4: the shapes do not penetrate at the middle of
                 * the interval, nor they do at the right limit of the interval.
                 * Thus, they will not collide in the current interval and in
                 * any of its sub-intervals. The pair is removed definitely from
                 * the colliding pairs. They collision, if any, will be detected
                 * in the following iteration(s).
                 */
                if (distanceRight > 0 && distanceMiddle > 0) {
                    iterator.remove();
                }
                /*
                 * Case #5 and #6: the shapes penetrate both at the middle and
                 * at the right limit of the interval. Thus, since the
                 * hypothesis is that they were not colliding at time zero, then
                 * the collision must occur in the left half of the interval.
                 * The pair is moved in the list leftMiddle and temporarily
                 * removed from pairs.
                 */
                if (distanceRight <= 0 && distanceMiddle <= 0) {
                    leftMiddleCollidingPairs.add(pair);
                    iterator.remove();
                }
            } // end of internal while
            /*
             * At this point no pairs are left in 'pairs'
             */
            if (leftMiddleCollidingPairs.size() != 0) {
                System.out.print(" left ");
                /*
                 * There are pairs colliding in the left half of the interval,
                 * so the ones colliding in the right half will be definitely
                 * lost and the limits of the interval are rewritten to its left
                 * half part.
                 */
                right = middle;
                middle = left + (right - left) / 2;
                /*
                 * Update control variables for saving calls to vclip
                 */
                firstCycle = false;
                useLastMiddleAsRight = true;
                useLastRight = false;
                /*
                 * Set the remaining pairs to work on
                 */
                pairs.addAll(leftMiddleCollidingPairs);
            } else if (middleRightCollidingPairs.size() != 0) {
                System.out.print(" right ");
                /*
                 * There are no pairs colliding in the left half of the
                 * interval, but there are ones colliding in the right half.
                 * Thus, all these will be retained to continue the search and
                 * the limits of the interval are rewritten to its right half
                 * part.
                 */
                left = middle;
                middle = left + (right - left) / 2;
                /*
                 * Update control variables for saving calls to vclip
                 */
                firstCycle = false;
                useLastMiddleAsRight = false;
                useLastRight = true;
                /*
                 * Set the pairs back to 'pairs' to continue working on them
                 */
                pairs.addAll(middleRightCollidingPairs);
            } else {
                /*
                 * There are no pairs colliding in the whole interval! This can
                 * happen only at the first iteration of the external while. In
                 * this case, by convention, the Double.POSITIVE_INFINITE
                 * constant is returned.
                 */
                System.out.println("\nNobody really colliding in this interval!");
                return Double.POSITIVE_INFINITY;
            }
            /*
             * This could happen when the external while continue to be executed
             * until the interval becomes of only one point or null. This should
             * normally never happen, but a control is put for safety.
             */
            if (right - left <= 0) {
                // Should never reach this point
                throw new IllegalStateException(
                        "Impossible to find a ftc in which no pair is penetrating; left = "
                                + left + " right = " + right);
            }
            /*
             * Control if the newely defined interval has reached the tolerance
             * length for which a "quasi"-collision time (a time in which the
             * shapes are not penetrating, but are so close each other that this
             * is an acceptable approximation of the contact time) can be
             * considered an exact collision time. In this case a tentative to
             * determine a first time of contact is made.
             */
            if (right - left <= EPSILON_TIME) {
                /*
                 * Determine the last left and right limits (they are the same
                 * for all the remaining pairs in 'pairs')
                 */
                double finalLeft = pairs.get(0).getLeft();
                double finalRight = pairs.get(0).getRight();
                /*
                 * Tentative ftc as the mid point of the last examined interval
                 */
                double ftc = finalLeft + (finalRight - finalLeft) / 2;
                /*
                 * Update colliding pairs information
                 */
                for (EntityPair collidingPair : pairs) {
                    DistanceReport drep = new DistanceReport();
                    drep.setFeaturePromotion(true, Math.toRadians(10));
                    // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
                    // distance
                    drep.setMaxClosePairs(1); // closest features
                    double distance = getDistanceAfterTime(ftc, collidingPair,
                            drep, ht);
                    collidingPair.setDistance(distance);
                    collidingPair.setDistanceReport(drep);
                }
                /*
                 * Check if there are still penetrating pairs at the tentative
                 * ftc
                 */
                List<EntityPair> stillPenetratingPairs = new ArrayList<EntityPair>();
                for (EntityPair collidingPair : pairs) {
                    if (collidingPair.getDistance() < 0)
                        stillPenetratingPairs.add(collidingPair);
                }
                /*
                 * This should be the case most of the times
                 */
                if (stillPenetratingPairs.size() == 0) {
                    System.out.println(" Ending... Ftc = " + ftc);
                    // OK!
                    return ftc;
                }
                /*
                 * Otherwise, the while will continue the search until it finds
                 * a tentative ftc in which no remaining pair is still
                 * penetrating
                 */
                System.out.println("\nTried to end! But a penetrating pair still"
                        + " existed! Continuing halving... ");
            }
        } // end external while
        return Double.POSITIVE_INFINITY;
    }

    /**
     * @param t
     * @param pair
     * @param drep
     * @param ht
     * @return
     */
    public static double getDistanceAfterTime(double t, EntityPair pair,
            DistanceReport drep, ClosestFeaturesHT ht) {
        // Backup of current transformation matrices
        ShapedEntity e1 = (ShapedEntity) pair.getE1();
        Matrix4d backupE1M = new Matrix4d(e1.getTransform());
        ShapedEntity e2 = (ShapedEntity) pair.getE2();
        Matrix4d backupE2M = new Matrix4d(e2.getTransform());
        // Move the shapes to time rightLimit
        e1.move(t);
        e2.move(t);
        // Calculate transformation matrix From e2's frame to e1's frame
        Matrix4d r2r1 = new Matrix4d();
        Matrix4d e1MInverse = new Matrix4d();
        inverseWithTranspose(e1.getTransform(), e1MInverse);
        r2r1.mul(e1MInverse, e2.getTransform());
        // Put both the entities back in the origin
        e1.getTransform().setIdentity();
        e2.getTransform().setIdentity();
        double distance = e1.vclip(drep, e2, r2r1, -1, ht);
        // Restore original e1's and e2's transformation matrices
        e1.getTransform().set(backupE1M);
        e2.getTransform().set(backupE2M);
        return distance;
    }

    /**
     * Calculate the transformation matrix that transforms from a reference
     * frame 2 to a reference frame 1, both frames given as transformation
     * matrices as well.
     * 
     * @param referenceFrame1TransformationMatrix
     *            the transformation matrix transforming to reference frame 1,
     *            left unchanged
     * @param referenceFrame2TransformationMatrix
     *            the transformation matrix transforming to reference frame 2,
     *            left unchanged
     * @param r2r1TransformationMatrix
     *            the transformation matrix transforming from reference frame 2
     *            to reference frame 1
     */
    public static void fromReferenceFrame2ToReferenceFrame1(
            Matrix4d referenceFrame1TransformationMatrix,
            Matrix4d referenceFrame2TransformationMatrix,
            Matrix4d r2r1TransformationMatrix) {
        Matrix4d inverse2 = new Matrix4d();
        inverseWithTranspose(referenceFrame2TransformationMatrix, inverse2);
        r2r1TransformationMatrix.mul(referenceFrame1TransformationMatrix,
                inverse2);
    }

    /**
     * Calculate the inverse of a transformation matrix 4x4 (composed of a
     * rotational part rot, which is a 3x3 matrix, a translational part t, which
     * is a 3 component vector, and the last row equal to 0 0 0 1) using the
     * property that this is always equal to the 4x4 matrix composed by the
     * transpose of rot, and the transformation of t by the negation of the
     * transpose of rot: t' = -rot^T * t. The last row is again 0, 0, 0, 1
     * 
     * @param original
     *            the original matrix to invert, left unchanged
     * @param inverted
     *            the inverted matrix
     */
    public static void inverseWithTranspose(Matrix4d original, Matrix4d inverted) {
        Matrix3d originalTransposeRot = new Matrix3d();
        original.get(originalTransposeRot);
        originalTransposeRot.transpose();
        inverted.setRotationScale(originalTransposeRot);
        inverted.m30 = 0;
        inverted.m31 = 0;
        inverted.m32 = 0;
        inverted.m33 = 1;
        Vector3d translationOriginal = new Vector3d();
        original.get(translationOriginal);
        originalTransposeRot.negate();
        originalTransposeRot.transform(translationOriginal);
        inverted.setTranslation(translationOriginal);
    }

    @Override
    public ActiveSite[] getContactSurfaces() {
        // TODO Auto-generated method stub
        return null;
    }

    @Override
    public List<EntityPair> getPairs() {
        return pairs;
    }

    @Override
    public double getFTC(List<EntityPair> pairs, double delta)
            throws SearchingErrorException {
        this.pairs = pairs;
        // the object pairs is manipulated in the static method, it will hold
        // the relevant elements after the function has returned
        return VClipCollisionDetectorTimeExitCondition.getFirstTimeOfContact(this.pairs, delta,
                this.ht);
    }

    // private static double getFirstTimeOfContact(List<EntityPair> pairs,
    // double delta, ClosestFeaturesHT ht) {
    // // Matrix4d e1M; // to manipulate transformation matrix of e1
    // // Matrix4d backupE1M; // to save and restore transformation matrix of
    // // e1
    // // Matrix4d e2M; // to manipulate transformation matrix of e2
    // // Matrix4d backupE2M; // to save and restore transformation matrix of
    // // e1
    //
    // // Remove all pairs that are not colliding at delta
    // Iterator<EntityPair> iterator = pairs.iterator();
    // while (iterator.hasNext()) {
    // EntityPair pair = iterator.next();
    // // Reset last distances
    // pair.setDistance(Double.NaN);
    // pair.setDistanceReport(null);
    // // Backup of current transformation matrices
    // ShapedEntity e1 = (ShapedEntity) pair.getE1();
    // Matrix4d backupE1M = new Matrix4d(e1.getTransform());
    // ShapedEntity e2 = (ShapedEntity) pair.getE2();
    // Matrix4d backupE2M = new Matrix4d(e2.getTransform());
    // // Move the shapes to time delta
    // e1.move(delta);
    // e2.move(delta);
    // // Calculate transformation matrix From e2's frame to e1's frame
    // Matrix4d m1 = new Matrix4d(e1.getTransform());
    // Matrix4d m2 = new Matrix4d(e2.getTransform());
    // Matrix4d r2r1 = new Matrix4d();
    // Matrix4d e1MInverse = new Matrix4d();
    // inverseWithTranspose(m1, e1MInverse);
    // r2r1.mul(e1MInverse, m2);
    // // Put both the entities back in the origin
    // e1.getTransform().setIdentity();
    // e2.getTransform().setIdentity();
    // // Creating a distanceReport for this vclip invocation, to be lost
    // DistanceReport drep = new DistanceReport();
    // drep.setFeaturePromotion(true, Math.toRadians(10));
    // // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal distance
    // drep.setMaxClosePairs(1); // closest features
    // // System.out.println("---- vclip invokation ----");
    // // System.out.println("RightLimit = delta = " + delta);
    // // System.out.println("e1 = " + e1.getName());
    // // System.out.println("e2 = " + e2.getName());
    // // System.out.println("Matrix21 = " + r2r1);
    // double distance = e1.vclip(drep, e2, r2r1, -1, ht);
    // // save vclip data into the pair, in any case!
    // pair.setDistance(distance);
    // pair.setDistanceReport(drep);
    // // System.out.println("Distance = " + distance);
    // // System.out.println("--------------------------");
    // // Restore original e1's and e2's transformation matrices
    // e1.getTransform().set(backupE1M);
    // e2.getTransform().set(backupE2M);
    // if (distance > 0) {
    // // e1 and e2 are not interpenetrating so they are removed from
    // // the list
    // iterator.remove();
    // }
    // }
    // // if nobody is left then return infinity
    // if (pairs.size() == 0) {
    // System.out.println("*** Nobody really colliding at this time! ***");
    // return Double.POSITIVE_INFINITY;
    // }
    // // Otherwise start the binary search of the first time of contact
    // // moving rightLimit backward, that is to say, starting from (0,delta/2]
    // // The invariant is that there are no colliding pairs at the time
    // // 'leftLimit'
    // // and there are at least a colliding pair before OR at the time
    // // 'rightLimit'
    // double leftLimit = 0;
    // // backup needed in case of moving leftLimit forward
    // double rightLimitBackup = delta;
    // double rightLimit = delta / 2;
    // int direction = BACKWARD_DIRECTION;
    // // backup needed in case of moving leftLimit forward
    // List<EntityPair> backupPairList = new ArrayList<EntityPair>();
    // backupPairList.addAll(pairs);
    // // Needed for handling rotation tunnelling or tunnelling in general
    // List<EntityPair> removedPairs = new ArrayList<EntityPair>();
    // while (rightLimit - leftLimit >= EPSILON_TIME) {
    // iterator = pairs.iterator();
    // while (iterator.hasNext()) {
    // EntityPair pair = iterator.next();
    // // Backup of current transformation matrices
    // ShapedEntity e1 = (ShapedEntity) pair.getE1();
    // Matrix4d backupE1M = new Matrix4d(e1.getTransform());
    // ShapedEntity e2 = (ShapedEntity) pair.getE2();
    // Matrix4d backupE2M = new Matrix4d(e2.getTransform());
    // // Move the shapes to time rightLimit
    // e1.move(rightLimit);
    // e2.move(rightLimit);
    // // Calculate transformation matrix From e2's frame to e1's frame
    // Matrix4d m1 = new Matrix4d(e1.getTransform());
    // Matrix4d m2 = new Matrix4d(e2.getTransform());
    // Matrix4d r2r1 = new Matrix4d();
    // Matrix4d e1MInverse = new Matrix4d();
    // inverseWithTranspose(m1, e1MInverse);
    // r2r1.mul(e1MInverse, m2);
    // // Put both the entities back in the origin
    // e1.getTransform().setIdentity();
    // e2.getTransform().setIdentity();
    // // Creating a distanceReport for this vclip invocation
    // DistanceReport drep = new DistanceReport();
    // drep.setFeaturePromotion(true, Math.toRadians(10));
    // // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
    // // distance
    // drep.setMaxClosePairs(1); // closest features
    // // System.out.println("---- vclip invokation ----");
    // // System.out.println("RightLimit = " + rightLimit);
    // // System.out.println("e1 = " + e1.getName());
    // // System.out.println("e2 = " + e2.getName());
    // // System.out.println("Matrix21 = " + r2r1);
    // double distance = e1.vclip(drep, e2, r2r1, -1, ht);
    // tunnellingCheck(distance, pair.getDistance(), direction);
    // // save vclip data into the pair
    // pair.setDistance(distance);
    // pair.setDistanceReport(drep);
    // // Restore original e1's and e2's transformation matrices
    // e1.getTransform().set(backupE1M);
    // e2.getTransform().set(backupE2M);
    // if (distance > 0) {
    // // e1 and e2 are not interpenetrating so they are removed
    // // from the
    // // list
    // iterator.remove();
    // }
    // }
    // // Check pairs
    // if (pairs.size() == 0) {
    // // nobody is colliding in the interval, so the leftLimit
    // // advance, and old rightLimit and the removed pairs are
    // // restored
    // System.out.println("---> Nobody Colliding in the interval ["
    // + leftLimit + " , " + rightLimit + "]");
    // leftLimit = rightLimit;
    // rightLimit = rightLimitBackup;
    // System.out.println("New interval [" + leftLimit + " , "
    // + rightLimit + "]<---");
    // pairs.addAll(backupPairList);
    // } else {
    // System.out.println("--->Somebody Colliding in the interval ["
    // + leftLimit + " , " + rightLimit + "]");
    // // rightLimit rewinds, saving backups
    // rightLimitBackup = rightLimit;
    // backupPairList.clear();
    // backupPairList.addAll(pairs);
    // // new rightLimit by halving
    // rightLimit = rightLimit - (rightLimit - leftLimit) / 2;
    // System.out.println("New interval [" + leftLimit + " , "
    // + rightLimit + "]<---");
    // }
    // }
    // // All objects left in 'pairs' represent shapes colliding at time
    // // rightLimit, which
    // // is considered the first time of contact
    // System.out.println("### Fine Calcolo FTC - Ftc = " + rightLimit);
    // return rightLimit;
    // }

}
